%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Edited by Shu Jiang, 2011.
% ALL RIGHTS ARE RESERVED.
% mailto: shujiang@tamu.edu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate all the outputs
% 2D with torso
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

addpath_output()
addpath('/home/shu/workspace/Research/output_v2/models/human/build_torso')
% name list 9 person
name  = {'Charles','Fred','Martin','Ram','Selina',... % 1st experiment
    'lily','po','ryan','vic'};  % 2nd experiment

% CONSTANT
% the number of subjects from the 1st experiment
exp1st = 5;
% the index of subjects use the right leg as swing leg during the step
rIndex = [6,7,9];
% experimental sampling frequency
fs = 480;

% initialization
time_rec = zeros(9,1); % record time of a step
sum_weight = 0;
sum_Lc = 0;
sum_Lt = 0;
sum_LT = 0;
model = 'HM';

for index = 1:length(name) % 5
    
    % get human data, with breaking pts for each step
    humanData = getData(name{1,index});
    
    sum_weight  = sum_weight+humanData.weight; % for average weight
    % breaking pts for each step
    StartPt = humanData.sns_brk(2);
    EndPt = humanData.sns_brk(3);
    
    % position
    if index <= exp1st
        [xpos,ypos,zpos] = getPosCartesian(humanData); % subjects from experiment 1st
    else
        [xpos,ypos,zpos] = getPosCartesianNew(humanData); % subjects from experiment 2st
    end
    
    
    % %     %% use the human mean data model
    % %     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % calculate length of each link
    % length of calf
    Lcl = cal_Lc(xpos.lankle_avg,ypos.lankle_avg,xpos.lknee,ypos.lknee);
    Lcr = cal_Lc(xpos.rankle_avg,ypos.rankle_avg,xpos.rknee,ypos.rknee);
    Lc = 1/2*(Lcl+Lcr);
    sum_Lc = sum_Lc+Lc;
    
    % length of thight;
    Ltl = cal_Lt(xpos.lhip,ypos.lhip,xpos.lknee,ypos.lknee);
    Ltr = cal_Lt(xpos.rhip,ypos.rhip,xpos.rknee,ypos.rknee);
    Lt = 1/2*(Ltl+Ltr);
    sum_Lt = sum_Lt+Lt;
    
    % length of torso
    LT = cal_Torso(xpos.hip_avg,ypos.hip_avg,xpos.torso,ypos.torso);
    sum_LT = sum_LT+LT;
    % %     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    
    %     DataLength = size(xpos.lknee);
    %%%%%%%%%%%  time  %%%%%%%%%%%%%%
    % scale time
    % from [c:d]
    ns_time_pre = (0:length(StartPt:EndPt)-1)'/fs;
    ns_ret = ns_time_pre;
    xpara_time = 1/ns_ret(end); % simply sacled time to 1
    ns_time = ns_ret*xpara_time;
    %%%% record time %%%%%%%%
    time_rec(index) = ns_ret(end);
    
    % other than mean human model
    % %     % amber model
    % %     Lc = AMBER_Lc;
    % %     Lt = AMBER_Lt;
    % %     LT = AMBER_LT;
    % calculate angle outputs
    if ismember(index,rIndex)
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,2);
    else
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,1);
    end
    
    % calculate slope outputs
    if ismember(index,rIndex)
        slope = Knee2D_slope_V2(xpos,ypos,zpos,Lc,Lt,LT,angle,humanData.sns_brk,2);
    else
        slope = Knee2D_slope_V2(xpos,ypos,zpos,Lc,Lt,LT,angle,humanData.sns_brk,1);
    end
    
    % calcualte position outputs
    if ismember(index,rIndex)
        pos = Knee2D_pos(xpos,Lc,Lt,StartPt, EndPt, angle,2);
    else
        pos = Knee2D_pos(xpos,Lc,Lt,StartPt, EndPt, angle,1);
    end
    
    %%%%%%%%%%%%%%%%%%% calculate the angle outputs  %%%%%%%%%%%%%%%%%%%%%
    % 1. hip angle
    [ave_dataHip,ave_timeHip] = BeamData(ns_time,angle.hip);
    ave_dataHip_s(index,:) =ave_dataHip;
    
    % 2. stance knee
    [ave_dataSK,ave_timeSK] = BeamData(ns_time,angle.sknee);
    ave_dataSK_s(index,:) = ave_dataSK;
 
    % 3. non-stance knee
    [ave_dataNSK,ave_timeNSK] = BeamData(ns_time,angle.nsknee);
    ave_dataNSK_s(index,:) = ave_dataNSK;
 
    % 4. torso hip angle
    [ave_dataTH,ave_timeTH] = BeamData(ns_time,angle.torso_hip);
    ave_dataTH_s(index,:) = ave_dataTH;
    
 
    %%%%%%%%%%%%%%%%%%% calculate the slope outputs  %%%%%%%%%%%%%%%%%%%%%
    q = [angle.sa;angle.sknee;angle.theta3;angle.theta4;angle.nsknee];
    outputs = cal_robot_para_18(q,model);
 
    % 1. hip position
    [ave_dataHP,ave_timeHP] = BeamData(ns_time,outputs.hippos);
    all_outputs. ave_dataHP_s(index,:) =ave_dataHP;
    %     outputs.delta_hippos(1)
    %     plot(ave_dataHP); hold on;
   
   
    % 2. linearized hip position
    [ave_dataHPL,ave_timeHPL] = BeamData(ns_time,outputs.delta_hippos);
    all_outputs.ave_dataHPL_s(index,:) =ave_dataHPL;
    %     plot(ave_dataHPL); hold on;
    
    % 3. non-stance slope
    [ave_dataNSslope,ave_timeNSslope] = BeamData(ns_time,outputs.nsslope);
    all_outputs.ave_dataNSslope_s(index,:) =ave_dataNSslope;
    
    % 4. linearized non-stance slope
    [ave_dataNSslopeL,ave_timeNSslopeL] = BeamData(ns_time,outputs.delta_nsslope);
    all_outputs.ave_dataNSslopeL_s(index,:) =ave_dataNSslopeL;
    %     plot(ave_dataNSslopeL); hold on;
    %     5. stance COM slope
    [ave_dataSCOM,ave_timeSCOM] = BeamData(ns_time, outputs.stCOM);
    all_outputs.ave_dataSCOM_s(index,:) = ave_dataSCOM;
    
    %     6. linearized stance COM slope
    [ave_dataSCOML,ave_timeSCOML] = BeamData(ns_time, outputs.delta_stCOM);
    all_outputs.ave_dataSCOML_s(index,:) = ave_dataSCOML;
end
% [x_meanCOM,upperBCOM,lowerBCOM] = meanValue(NAO_com_s);
ave_time = ave_timeHP;
%% calculate the mean value and save
%%%%%%%%%%%% angles %%%%%%%%%%%%%%%%%%%%%%%%%
% 1. hip angle
[x_meanHip,upperBHip,lowerBHip] = meanValue(ave_dataHip_s);

% 2. stance knee
[x_meanSK,upperBSK,lowerBSK] = meanValue(ave_dataSK_s);
%     plot(ave_dataSK); hold on;

% 3. non-stance knee
[x_meanNSK,upperBNSK,lowerBNSK] = meanValue(ave_dataNSK_s);

% 4. torso hip angle
[x_meanTH,upperBTH,lowerBTH]=meanValue(ave_dataTH_s);

%%%%%%%%%%%%%%%% slopes %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[Xmean,XupperB,XlowerB] = cal_mean_outputs(all_outputs);

%% save the data
 ns_time_mean = mean(time_rec)*ave_time'; % averaged time
 
 data_name = 'mean_data';
 folder_name = ['data_' model '_18'];
 save_outputs(Xmean,ns_time_mean,data_name,folder_name);
%%%%%%%%%%%% angles %%%%%%%%%%%%%%%%
ns_time = ns_time_mean;
 % 1. hip angle
hip_angle =x_meanHip';
save(['./', folder_name, '/',data_name,'_hipAngle.mat'],'hip_angle','ns_time')

 % 3. non-stance knee angle
ns_knee = x_meanNSK';
save(['./', folder_name, '/', data_name,'_nsknee.mat'],'ns_knee','ns_time');

% 4. stance knee angle
s_knee = x_meanSK';
save(['./', folder_name, '/', data_name,'_sknee.mat'],'s_knee','ns_time');

% 5. torso angle
torso_angle =x_meanTH';
save(['./', folder_name, '/',data_name, '_TorsoHipAngle.mat'],'torso_angle','ns_time');